package com.webcamtracker;

import Jama.Matrix;


public class KalmanFilter {
    public static void main(String[] args) {
        // function kalman(alpha, duration, dt) - Kalman filter simulation
        // alpha = forgetting factor (alpha >= 1)
        // duration = length of simulation (seconds)
        // dt = step size (seconds)
        // Copyright 1998 Innovatia Software.  All rights reserved.
        // http://www.innovatia.com/software

        double measnoise = 10; // position measurement noise (feet)
        double accelnoise = 0.5; // acceleration noise (feet/sec^2)
        Matrix c = new Matrix(new double[][]{{1.0, 0.0}});// measurement matrix
        Matrix x = new Matrix(new double[][]{{0.0}, {0.0}});// measurement matrix
        Matrix xhat = x.copy();

        double dt = 7;

        //l Simulate the process
        Matrix ProcessNoise = new Matrix(new double[][]{{(Math.pow(dt, 2) / 2) * Math.random()}, {dt * Math.random()}}).times(2);
        ProcessNoise.print(0, ProcessNoise.rank());
        //l Simulate the measurement
        double MeasNoise = measnoise * Math.random();
        // transition matrix
        Matrix a = new Matrix(new double[][]{{1, dt}, {0, 1}});
        // process noise covariance
        Matrix Q = new Matrix(new double[][]{{Math.pow(dt, 4) / 4, Math.pow(dt, 3) / 2}, {Math.pow(dt, 3) / 2, Math.pow(dt, 2)}}).times(Math.pow(accelnoise, 2));
        // initial estimation covariance
        Matrix P = Q.copy();
        //measurement error covariance
        Matrix R = new Matrix(new double[][]{{Math.pow(measnoise, 2)}});
        x = (a.times(x)).plus(ProcessNoise);

        //z = c * x + MeasNoise;
        Matrix z = c.times(x).plus(new Matrix(new double[][]{{MeasNoise}}));
        //l Innovation
        //Inn = z - c * xhat;
        Matrix Inn = z.minus(c.times(xhat));
        //l Covariance of Innovation
        Matrix c2 = c.transpose();
        Matrix a2 = a.transpose();
        Matrix s = (c.times(P).times(c2)).plus(R);
        //l Gain matrix
        //K = a * P * c' * inv(s);
        Matrix K = a.times(P).times(c2).times(s.inverse());
        //l State estimate
        //xhat = a * xhat + K * Inn;
        xhat = (a.times(xhat)).plus(K.times(Inn));
        //l Covariance of prediction error
        //  P = a * P * a' + Q - a * P * c' * inv(s) * c * P * a';
        P = (a.times(P).times(a2)).plus(Q).minus(a.times(P).times(c2).times(s.inverse()).times(c).times(P).times(a2));
        P.print(0, P.rank());
        xhat.print(0, xhat.rank());
        final double v = x.get(0, 0);
        System.out.println("v = " + v);


    }
}

/*
function kalman(alpha, duration, dt)

% function kalman(alpha, duration, dt) - Kalman filter simulation
% alpha = forgetting factor (alpha >= 1)
% duration = length of simulation (seconds)
% dt = step size (seconds)
% Copyright 1998 Innovatia Software.  All rights reserved.
% http://www.innovatia.com/software

measnoise = 10; % position measurement noise (feet)
accelnoise = 0.5; % acceleration noise (feet/sec^2)

a = [1 dt; 0 1]; % transition matrix
c = [1 0]; % measurement matrix
x = [0; 0]; % initial state vector
xhat = x; % initial state estimate

Q = accelnoise^2 * [dt^4/4 dt^3/2; dt^3/2 dt^2]; % process noise covariance
P = Q; % initial estimation covariance
R = measnoise^2; % measurement error covariance

% set up the size of the innovations vector
Inn = zeros(size(R));

pos = []; % true position array
poshat = []; % estimated position array
posmeas = []; % measured position array

Counter = 0;
for t = 0 : dt: duration,
  Counter = Counter + 1;
  % Simulate the process
  ProcessNoise = accelnoise * [(dt^2/2)*randn; dt*randn];
  x = a * x + ProcessNoise;
  % Simulate the measurement
  MeasNoise = measnoise * randn;
  z = c * x + MeasNoise;
  % Innovation
  Inn = z - c * xhat;
  % Covariance of Innovation
  s = c * P * c' + R;
  % Gain matrix
  K = a * P * c' * inv(s);
  % State estimate
  xhat = a * xhat + K * Inn;
  % Covariance of prediction error
  P = a * P * a' + Q - a * P * c' * inv(s) * c * P * a';
  % Save some parameters in vectors for plotting later
  pos = [pos; x(1)];
  posmeas = [posmeas; z];
  poshat = [poshat; xhat(1)];
end

% Plot the results
t = 0 : dt : duration;
t = t';
plot(t,pos,'r',t,poshat,'g',t,posmeas,'b');
grid;
xlabel('Time (sec)');
ylabel('Position (feet)');
title('Kalman Filter Performance');*/
